pi = 4*atan(1);

A=[
-609.0   245.3   280.0;
-628.0   280.0   310.0;
-605.0   258.0   303.0;
-621.0   314.0   301.0;
-645.0   283.5   301.0;
-615.0   290.0   302.0;
671.0  -277.0  -339.0;
577.0  -301.0  -251.0;
650.0  -308.0  -306.0;
617.0  -289.0  -285.0;
637.0  -287.0  -325.0;
578.0  -256.6  -263.0;
];

B=[
279.0  -619.0   264.0;
275.0  -593.0   267.0;
313.0  -636.0   285.0;
333.0  -648.0   335.0;
249.0  -575.0   248.0;
282.0  -624.0   298.0;
-268.0   599.0  -242.0;
-277.0   637.0  -304.0;
-285.0   598.0  -270.0;
-320.0   655.0  -303.0;
-271.0   599.0  -288.0;
-334.0   639.0  -332.0;
];

C=[
274.0   280.0  -612.0;
302.0   288.0  -615.0;
298.0   303.0  -615.0;
308.0   318.0  -637.0;
260.0   276.0  -594.0;
321.0   305.0  -623.0;
-311.0  -306.0   652.0;
-270.0  -261.0   560.0;
-302.0  -325.0   631.0;
-290.0  -308.0   631.0;
-328.0  -317.0   643.0;
-297.0  -278.0   596.0;
];

R=[
972.0  1004.0   912.0;
2054.0  2025.0  1966.0;
3053.0  3111.0  2999.0;
4041.0  4152.0  3935.0;
5099.0  5186.0  4999.0;
6067.0  6241.0  5955.0;
-1007.0 -1026.0  -971.0;
-2060.0 -2088.0 -1978.2;
-3172.0 -3198.0 -3034.0;
-4138.0 -4108.0 -3982.0;
-5125.0 -5187.0 -4967.0;
-6139.0 -6221.0 -5991.0;
];


//A
iA=[
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
]*10; //cm->mm 

iB=[
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
]*10; //cm->mm

theta=-2*pi/3;
irB=iB*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

iC=[
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
-1 0 0;
]*10; //cm->mm

theta=-4*pi/3;
irC=iC*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

//R
iR=[
0 0 5;
0 0 10;
0 0 16;
0 0 20;
0 0 26;
0 0 27;
0 0 -5;
0 0 -9;
0 0 -15;
0 0 -20;
0 0 -25;
0 0 -30;
]*pi/180;

mult = 2^14;

Moutput = [A;B;C;R];
Minput = [iA;irB;irC;iR]*mult;

[arc,la,lb,sig,resid] = armax( 0, 0, Moutput', Minput');

[Ax,Bx,Dx]=arma2p(arc);   //Results in polynomial form. 

M = pinv(coeff(Bx));

// show matrix
M

u = mopen('matrix.h','w')

fprintf(u,'double hrobot_adnsMatrix[3][3] = {\n');
for j=1:3 
  fprintf(u,'{');
  for i=1:3
    fprintf(u,'%f',M(j,i));
    if(i ~= 3),
      fprintf(u,',');
    end
  end
  fprintf(u,'}');
  if(j ~= 3),
    fprintf(u,',');
  end
  fprintf(u,'\n');
end
fprintf(u,'};\n');

mclose(u);
